/**********************************************************************
 Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved.
***********************************************************************/
#ifndef FREESTAND_H
#define FREESTAND_H

#include "FSM/FSMState.h"

class State_FreeStand : public FSMState{
public:
    State_FreeStand(CtrlComponents *ctrlComp);
    ~State_FreeStand(){}
    void enter();
    void run();
    void exit();
    FSMStateName checkChange();
private:
    Vec3 _initVecOX;    //p_b0, 足端位置计算用(用于构建HTM)
    Vec34 _initVecXP;   //4个p_si, 足端位置计算用(p_bi=T*p_si)
    float _rowMax, _rowMin;
    float _pitchMax, _pitchMin;
    float _yawMax, _yawMin;
    float _heightMax, _heightMin;


    /// @brief 由{body}姿态和高度变化计算{body}下足端位置坐标
    /// @param row 机体翻滚角
    /// @param pitch 机体俯仰角
    /// @param yaw 机体偏航角
    /// @param height 当前高度与初始化高度之差
    /// @return 四条腿足端位置坐标
    Vec34 _calcOP(float row, float pitch, float yaw, float height);
    void _calcCmd(Vec34 vecOP);
};

#endif  // FREESTAND_H